#include "mpu6050_dmp.h"
#include "gpio.h"
#include "math.h"

// IIC通信引脚配置
#define SDA_MPU6050 GPIO_PIN_21
#define SCL_MPU6050 GPIO_PIN_22

// 定义变量
MPU6050 mpu;
float Yaw_dmp,Pitch_dmp,Roll_dmp;
int16_t gx_dmp,gy_dmp,gz_dmp;

// 中断引脚配置
int const INTERRUPT_PIN = GPIO_PIN_19;         // Define the interruption #0 pin

/*---MPU6050 Control/Status Variables---*/
bool DMPReady = false;  // Set true if DMP init was successful
uint8_t MPUIntStatus;   // Holds actual interrupt status byte from MPU
uint8_t devStatus;      // Return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // Expected DMP packet size (default is 42 bytes)
uint8_t FIFOBuffer[64]; // FIFO storage buffer

/*---Orientation/Motion Variables---*/ 
Quaternion q;           // [w, x, y, z]         Quaternion container
VectorFloat gravity;    // [x, y, z]            Gravity vector
float ypr[3];           // [yaw, pitch, roll]   Yaw/Pitch/Roll container and gravity vector

/*------Interrupt detection routine------*/
volatile bool MPUInterrupt = false;        // Indicates whether MPU6050 interrupt pin has gone high
void DMPDataReady() {
  MPUInterrupt = true;
}

// 移植了DMP库的MPU6050初始化
void MPU6050_AND_DMP_Init(void)
{
    // 初始化IIC
    Wire.begin(SDA_MPU6050,SCL_MPU6050);
    Wire.setClock(400000); // 400kHz I2C clock. Comment on this line if having compilation difficulties

    // 开机初始化工作
    mpu.initialize();

    // 初始化中断(可不要)
    pinMode(INTERRUPT_PIN, INPUT);

    // 连接确认
    if(mpu.testConnection() == false){
        while(true);
    }

    // dmp初始化（返回0即为成功）
    devStatus = mpu.dmpInitialize();

    // 去零漂
    mpu.setXGyroOffset(0);
    mpu.setYGyroOffset(0);
    mpu.setZGyroOffset(0);
    mpu.setXAccelOffset(0);
    mpu.setYAccelOffset(0);
    mpu.setZAccelOffset(0);

    // 执行内部任务
    if (devStatus == 0) 
    {
        mpu.CalibrateAccel(6);  // Calibration Time: generate offsets and calibrate our MPU6050
        mpu.CalibrateGyro(6);
    
        mpu.PrintActiveOffsets();
        mpu.setDMPEnabled(true);

        /*Enable Arduino interrupt detection*/
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), DMPDataReady, RISING);
        MPUIntStatus = mpu.getIntStatus();

        /* Set the DMP Ready flag so the main loop() function knows it is okay to use it */
        DMPReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize(); //Get expected DMP packet size for later comparison
    } 
}

void MPU6050_DMP_GetEuler(void)
{
    mpu.getRotation(&gx_dmp, &gy_dmp, &gz_dmp);

    if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) 
    {
        // Get the Latest packet 
        /* Display Euler angles in degrees */
        mpu.dmpGetQuaternion(&q, FIFOBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        Yaw_dmp   = ypr[0] * 180 / M_PI;

        Pitch_dmp = ypr[1] * 180 / M_PI;

        Roll_dmp  = ypr[2] * 180 / M_PI;
        /* Display Euler angles in degrees */
    }
}